#include <core/execution.h>
#include <localization/maps/lamps_maps.h>

void ExecuteDeliveryRobot() {
  player_point_2d initial_position;
  double initial_yaw;

  // ------------------------------- User Input ------------------------------
  cout << endl << "Initial pose (px py pa), 'pa' in degrees: ";
  cin >> initial_position.px >> initial_position.py >> initial_yaw;
  initial_yaw = dtor(initial_yaw);
  cout << endl;
  sleep(3);

  // ----------------------------- Player Setup ------------------------------
  PlayerCc::PlayerClient player_client("localhost");
  PlayerCc::Position2dProxy position2d_proxy(&player_client);
  PlayerCc::SonarProxy sonar_proxy(&player_client);

  position2d_proxy.RequestGeom();
  position2d_proxy.ResetOdometry();
  sonar_proxy.RequestGeom();
  player_client.Read();
  player_client.Read();
  position2d_proxy.SetMotorEnable(1);

  // ------------------------- Synchronized Objects --------------------------
  gsl_matrix *initial_covariance = gsl_matrix_alloc(3, 3);
  gsl_matrix_set_zero(initial_covariance);
  gsl_matrix_set(initial_covariance, 0, 0, 0.1);
  gsl_matrix_set(initial_covariance, 1, 1, 0.1);
  gsl_matrix_set(initial_covariance, 2, 2, 0.001);

  SyncedCurrentPose synced_current_pose;
  SyncedCurrentMission synced_current_mission;
  player_pose2d initial_pose = {initial_position.px, initial_position.py, initial_yaw};
  synced_current_pose.set_current_pose(initial_pose);
  SyncedCurrentCovariance synced_current_covariance;
  synced_current_covariance.set_current_covariance(initial_covariance);
  SyncedLampCentroids synced_lamp_centroids;

  // ------------------------ Localization and Vision ------------------------
  LocalizationEstimator localization_estimator(&initial_pose,
            initial_covariance, MappedLampsBBlock(), &player_client, &position2d_proxy,
            &synced_current_pose, &synced_current_covariance, &synced_lamp_centroids);
  VisionSystem vision_system(&synced_lamp_centroids);

  gsl_matrix_free(initial_covariance);

  boost::thread localization_estimator_thread(boost::ref(localization_estimator));
  boost::thread vision_system_thread(boost::ref(vision_system));

  // -------------------- GUI Server -------------------
  GUIServer gui_server(&synced_current_pose,
    				   &synced_current_mission);
  boost::thread gui_server_thread(boost::ref(gui_server));

  // -------------------- Reactive, Planner and Controller -------------------
  ReactiveBehavior reactive_behavior(&player_client, &position2d_proxy,
          &sonar_proxy, &synced_current_pose);
  PathPlanner path_planner("izmap", "tmap");
  Controller controller(&path_planner,
		  	  	  	  	&reactive_behavior,
		  	  	  	    &synced_current_pose,
		  	  	  	    &synced_current_mission);
  boost::thread controller_thread(boost::ref(controller));

  controller_thread.join();
}
